#include "FreeRTOS.hpp"
#include "Flash.hpp"
#include "PID.hpp"
#include "Tools.hpp"


typedef void (*app_ptr)(void);
extern uint16_t TxBuf[AXIS_BUFFER][4];
TaskHandle_t Hid_Send_Task_handler;
TaskHandle_t Wheel_Task_handler;
TaskHandle_t DefaultTasks_handler;
TaskHandle_t Config_auto_save_handler;
void Hid_Send_Task(void *pvParameters);
void Wheel_Task(void *pvParameters);
void config_auto_save(void *pvParameters);
typedef struct {
  out_reprot control;
  Wheel_config wheel;
} Config;
Config save_config;
uint16_t wawe_value;
out_reprot settings = {0x0d, 0, 100, 0, 100, 0, 100, 0, 100, 15, 1,
                       1,    1, 1,   1, 0,   0, 0,   0, 0,   0,  0};
Wheel_config wheel_config = {0x0e,  0x00, 50, 0,  0, 0, 0,
                             16384, 720,  50, 50, 0, 0, 0};
Encoder *encoder;
Time *time;
void OSDelay(uint16_t time_ms) { vTaskDelay(time_ms); }
extern "C" void DefaultTasks(void *pvParameters) {
  // vTaskDelay(4000);
  time = new Time();
  Flash *flash = new Flash();
  // flash->SaveConfig((uint32_t *)&settings,12);
  // printf("??\n");
  flash->ReadConfig((uint32_t *)&save_config, sizeof(Config));
  if (save_config.control.reprotid == 0x0d) {
    settings = save_config.control;
    wawe_value = settings.wave_filtering > 99 ? 99 : settings.wave_filtering;
    // settings = save_config.control;
  }
  if (save_config.wheel.reportid == 0x0e) {
    // memcpy(&wheel_config,&save_config.wheel,sizeof(Wheel_config));
    // TIM1 初始化 编码器模式
    wheel_config = save_config.wheel;
    encoder = new Encoder(wheel_config.Wheel_EncoderCPR);
  } else {
    encoder = new Encoder(wheel_config.Wheel_EncoderCPR);
  }
  // USB_Printf("%d,%d,%d\n",save_config.control.reprotid,save_config.wheel.Wheel_offset,save_config.wheel.Wheel_EncoderCPR);
  delete flash;
  if (xTaskCreate((TaskFunction_t)Hid_Send_Task, (const char *)"Hid_Send_Task",
                  (uint16_t)512, (void *)NULL, (UBaseType_t)4,
                  (TaskHandle_t *)&Hid_Send_Task_handler) != pdPASS) {
    printf("Hid_Send_Task created as there was insufficient heap memory "
           "remaining.\r\n");
  } else {
    printf("Hid_Send_Task task was created successfully.\r\n");
  };
  // vTaskDelay(5000);
  if (Calibrate(5000)) {

    if (xTaskCreate((TaskFunction_t)Wheel_Task, (const char *)"Wheel_Task",
                    (uint16_t)512, (void *)NULL, (UBaseType_t)5,
                    (TaskHandle_t *)&Wheel_Task_handler) != pdPASS) {
      printf("Wheel_Task created as there was insufficient heap memory "
             "remaining.\r\n");
    } else {
      printf("Wheel_Task task was created successfully.\r\n");
    };
  }
#ifdef AT32
  while (1) {
    vTaskDelay(1);
  }
#endif
  vTaskDelete(DefaultTasks_handler);
}
Dead_Axis dead = {0};
reprot_type buf = {0};
uint8_t MODE = 1;

void configchange(out_reprot *new_settings) {
  // settings = *new_settings;
  Axis_Init_Config(&dead, new_settings);
  wawe_value =
      new_settings->wave_filtering > 99 ? 99 : new_settings->wave_filtering;
  MODE = new_settings->MODE_Flags;
  switch (new_settings->temp) {
  case 0x01:
    Flash::Clean_Flash_Key_Reset();
    break;
  case 0x0f:
    save_config.control = *new_settings;
    save_config.wheel = wheel_config;
#ifndef AT32
    if (xTaskCreate((TaskFunction_t)config_auto_save,
                    (const char *)"Config_auto_save", (uint16_t)512,
                    (void *)&save_config, (UBaseType_t)6,
                    (TaskHandle_t *)&Config_auto_save_handler) != pdPASS) {
      printf("Config Save filed\r\n");
    } else {
      printf("Config Save\r\n");
    };
    return;
#endif
    config_auto_save(&save_config);
    break;
  default:
    break;
  }

  // printf("%d %d %d %d
  // %d\n",settings.MODE_Flags,settings.dead_Brake_High,settings.dead_Brake_Low,settings.ENABLE_Brake,settings.wave_filtering);
}
#ifdef AT32
void calSensor() {
  uint16_t offset = 0;
  {
    for (int i = 0; i < 4095; i++) {
      dac_1_data_set(DAC1_12BIT_RIGHT, i);
      if (TxBuf[3][0] < 100)
        offset = i;
      if (TxBuf[3][1] < 100)
        offset = i;
      if (TxBuf[3][2] < 100)
        offset = i;
      if (TxBuf[3][3] < 100)
        offset = i;
      vTaskDelay(1);
    }
  }
  dac_1_data_set(DAC1_12BIT_RIGHT, offset);
  vTaskDelay(10);
  dead.Axis_Rx_Low = TxBuf[3][0] << 4;
  dead.Axis_Ry_Low = TxBuf[3][1] << 4;
  dead.Brake_Low = TxBuf[3][2] << 4;
  dead.Throttle_Low = TxBuf[3][3] << 4;
}
#endif

void Hid_Send_Task(void *pvParameters) {
  // vTaskDelay(200);
  ADC_Start();
  Button *gpio = new Button();
  uint8_t i = 0;
  uint32_t Axis_Rx = 0;
  uint32_t Axis_Ry = 0;
  uint32_t Brake = 0;
  uint32_t Throttle = 0;
  Axis_Init_Config(&dead, &settings);
  Kalman_Filter_Par Kalman_Filter_Par_A = {0.02, 0, 0, 0, 0.001, 0.543};
  LowPassFilter *lpf_brake = new LowPassFilter(0.02f);
  LowPassFilter *lpf_throttle = new LowPassFilter(0.02f);
  LowPassFilter *lpf_rx = new LowPassFilter(0.02f);
  LowPassFilter *lpf_ry = new LowPassFilter(0.02f);
  while (1) {
    buf.Buttons = 0;
    buf.HatSwitch = 0;
    buf.reprotid = 0x01;
    Axis_Rx = 0;
    Axis_Ry = 0;
    Throttle = 0;
    Brake = 0;
    for (int i = 0; i < AXIS_BUFFER; i++) {
      Axis_Rx += TxBuf[i][0];
      Axis_Ry += TxBuf[i][1];
      Brake += TxBuf[i][2];
      Throttle += TxBuf[i][3];
    }
    buf.Axis_Rx = Axis_Rx / AXIS_BUFFER;
    buf.Axis_Ry = Axis_Ry / AXIS_BUFFER;
    buf.Brake = Brake / AXIS_BUFFER;
    buf.Throttle = Throttle / AXIS_BUFFER;
    // buf.Axis_Rx = Axis_Rx;
    // buf.Axis_Ry = Axis_Ry;
    // buf.Brake = Brake;
    // buf.Throttle = Throttle;
    if (!gpio->ReadButton(5)) {
      buf.HatSwitch = 0x05;
    }

    if (!gpio->ReadButton(1)) {

      buf.HatSwitch = 0x01;
    }
    if (!gpio->ReadButton(7)) {

      buf.HatSwitch = 0x07;
    }
    if (!gpio->ReadButton(0)) {
      vTaskDelay(200);
      if (!gpio->ReadButton(0)) {
        buf.Buttons = 0x01 << i;
        i++;
        if (i > 16) {
          i = 0;
        }
      }
    }
    if (!gpio->ReadButton(3)) {
      buf.HatSwitch = 0x03;
    }
    Axis_X4(&buf);
    Axis_Config(&buf, &dead);
    // printf("mode: %d \n" , MODE);
    switch (MODE) {
    case 1:
      Amplitude_Filter(&buf, wawe_value);
      break;
    case 2:
      Filter(&buf, wawe_value);
      break;
    case 4:
      buf.Brake = lpf_brake->operator()(buf.Brake);
      buf.Throttle = lpf_throttle->operator()(buf.Throttle);
      buf.Axis_Ry = lpf_ry->operator()(buf.Axis_Ry);
      buf.Axis_Rx = lpf_rx->operator()(buf.Axis_Rx);
      break;
    case 8:
      Kalman_Filter_Par_A.Kg = wawe_value / 100.0f;
      buf.Axis_Ry = KalmanFilter(&Kalman_Filter_Par_A, buf.Axis_Ry);
      break;
    default:
      break;
    }
    // USB_Printf("%f, %f,%f,%f, %f, %f ,%f ,%f,%f,%d\n",
    // clark.Ia,clark.Ib,clark.Ic,clark.I_alpha,clark.I_beta,Park.Id,Park.Iq,iq,engle,whell_temp);
    // USB_Printf("%d, %d, %d,
    // %d\n",buf.Axis_Rx,buf.Axis_Ry,buf.Brake,buf.Throttle);
    Send_report(&buf, 20);
    vTaskDelay(1);
  }
}

float engle_offset = 0;
extern uint16_t cc_irq;
extern uint32_t Start_Flist[2];

void wheel_config_changed(Wheel_config *wheel_config_t) {
  memcpy(&wheel_config, wheel_config_t, sizeof(Wheel_config) - 2);
  if (wheel_config_t->Wheel_Flag == 0x01) {
    engle_offset =
        wheel_config_t->Wheel_Engle / 1.0f / wheel_config_t->Wheel_Degrees;
    engle_offset *= 65535;
    wheel_config_t->Wheel_Flag = 0;
    // USB_Printf("%f,%d,%d\n", engle_offset, wheel_config_t->Wheel_Engle,
    // wheel_config_t->Wheel_Degrees);
  };
  if (wheel_config_t->Wheel_Flag == 0x44) {
    wheel_config.Wheel_offset = encoder->getRawEncoder(true) - Start_Flist[1];
    wheel_config_t->Wheel_Flag = 0;
    // USB_Printf("%f,%d,%d\n", engle_offset, wheel_config_t->Wheel_Engle,
    // wheel_config_t->Wheel_Degrees);
  }
  // USB_Printf("%d %d %d %d %d
  // %d\n",wheel_config.Wheel_Degrees,wheel_config.Wheel_EncoderCPR,wheel_config.Force,wheel_config.Wheel_Flag,wheel_config.Wheel_Engle,wheel_config.Friction);
}
extern volatile int16_t Cir;
void Wheel_Task(void *pvParameters) {
  vTaskDelay(200);
  float wheel = 1440.0f;
  int32_t whell_temp = 0;
  // Cir = 0;
  // vTaskDelete(Wheel_Task_handler);
  PIDControl *pid_q = new PIDControl(5, 0.1f, 0, 8.0f, 15.0f);
  PIDControl *pid_pos = new PIDControl(0.001f, 0.05f, 0.07f, 1, 2);
  // uint8_t buffer[3] = {0x02, 0x00, 0x85};
  PIDControl *pid_d = new PIDControl(1, 0.1f, 0, 1, 5);
  float engle;
  float forck_iq = 0;
  float forck_id = 0;
  // float pos_set = 0;
  ClarkeParame clark = {0};
  ParkParame Park = {0};
  Current_P Curren = ADC_GetValue();
  LowPassFilter *lpf_d = new LowPassFilter(0.005f);
  LowPassFilter *lpf_q = new LowPassFilter(0.005f);
  LowPassFilter *lpf_speed = new LowPassFilter(0.005f);
  uint8_t dir_force = 1;
  int i = 0;
  // float j = 0;
  while (1) {
    engle = encoder->getDegEncoder();
    whell_temp = (GetRawWhell(0) + wheel_config.Wheel_offset) * wheel /
                 wheel_config.Wheel_Degrees; // 1304;
    buf.Wheel = whell_temp;
    if (whell_temp >= 32767) {
      buf.Wheel = 32767;
    } else if (whell_temp <= -32768) {
      buf.Wheel = -32768;
    }
    ADC_GetValue();
    clark.Ia = Curren->Ia;
    clark.Ib = Curren->Ib;
    clark.Ic = Curren->Ic;
    ClarkeTransformation(&clark);
    Park.I_alpha = clark.I_alpha;
    Park.I_beta = clark.I_beta;
    Park.Angle_Cos = cosf(engle);
    Park.Angle_Sin = sinf(engle);
    ParkTransformation(&Park);
    i++;
    if (abs(whell_temp - engle_offset) && i > 3) {

      if (froce.Flag) {
        if (dir_force) {
          forck_iq = froce.Froce * 0.00025f * wheel_config.Force / 100;
          if (whell_temp <= 100 && whell_temp > -100) {
            if (froce.Froce < 0) {
              dir_force = 0;
            }

            forck_iq = forck_iq * whell_temp / 100.0f;
          } else if (whell_temp < -50) {
            forck_iq = -forck_iq;
          }
          // USB_Printf("%d %f\n",froce.Froce,forck_iq);
        } else {
          forck_iq = froce.Froce * 0.00035f * wheel_config.Force / 100;
        }
      } else {
        dir_force = 1;
        pid_pos->Outputlimit = wheel_config.Wheel_center / 25.0f;
        pid_pos->Integerrlimit = pid_pos->Outputlimit / 2;
        forck_iq = pid_pos->operator()((whell_temp - engle_offset));
        // printf("1:%f\n", forck_iq);
        i = 0;
      }
      if (whell_temp >= 32767) {
        forck_iq += wheel_config.Wheel_stop / 25.0f;
      } else if (whell_temp <= -32768) {
        forck_iq -= wheel_config.Wheel_stop / 25.0f;
      }
      // printf("2:%f\n", forck_iq);
      forck_iq += lpf_speed->operator()(encoder->getRotspeed()) *
                  (wheel_config.Friction - (wheel_config.Antigravity >> 3)) /
                  -50;
      // printf("3:%f\n", forck_iq);
    }
    // j+= wheel_config.Antigravity / 250.0f;
    // forck_iq = sinf(j) * wheel_config.Friction / 20.0f;
    buf.Force = forck_iq * 1000;
    Park.Iq = lpf_q->operator()(Park.Iq);
    Park.Id = lpf_d->operator()(Park.Id);
    float iq = pid_q->operator()((Park.Iq - forck_iq));
    float id = pid_d->operator()((Park.Id - forck_id));
    // USB_Printf("%f,%f,%f\n", clark.Ia, clark.Ib, clark.Ic);
    Setdq(iq, id);
    vTaskDelay(1);
  }
}

void config_auto_save(void *pvParameters) {
  Flash *flash = new Flash();
  // flash->EraseConfig();
  flash->SaveConfig((uint32_t *)pvParameters, sizeof(Config));
  // USB_Printf("%d\n",((Config *)pvParameters)->wheel.Wheel_offset);
  delete flash;
#ifndef AT32
  vTaskDelete(Config_auto_save_handler);
#endif
}

void RTOS_TaskCreate() {
  taskENTER_CRITICAL();
  if (xTaskCreate((TaskFunction_t)DefaultTasks, (const char *)"DefaultTasks",
                  (uint16_t)512, (void *)NULL, (UBaseType_t)1,
                  (TaskHandle_t *)&DefaultTasks_handler) != pdPASS) {
    printf("DefaultTasks created as there was insufficient heap memory "
           "remaining.\r\n");
  } else {
    printf("DefaultTasks task was created successfully.\r\n");
  };
  taskEXIT_CRITICAL();
#ifndef CH32
  __set_PRIMASK(0);
#endif
}

void RTOS_Start() { vTaskStartScheduler(); }